/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
# example client reading laser scans with swig wrapped c++ library in python
import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
#sys.path.append("../../../interfaces/common")
#sys.path.append("../../../GIMI")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import RangingClient
import gimi
if sys.argv.__len__() != 3:
	print "Usage: python %s clientName serverName"%sys.argv[0]
	sys.exit()
g = gimi.GIMI()
rc = RangingClient.CRangingClient(g, 0)
print "connecting... ",
ret = g.connectToHubEx("asrobo.hut.fi", 40002, sys.argv[1])
print RangingClient.GetErrorStr(ret)
if ret != RangingClient.KMaCIOK: sys.exit()
print "set target"
if sys.argv[2].find(".") < 0:
	print sys.argv[2]
	rc.SetDefaultTarget(sys.argv[2])
else:
	print "with fancy name ", sys.argv[2]
	rc.SetDefaultTarget(RangingClient.SMaCISL(sys.argv[2]))

print "open... ",
ret = rc.Open()
print RangingClient.GetErrorStr(ret)
if RangingClient.GetErrorStr(ret) != "OK": sys.exit()
tStamp = RangingClient.TTimestamp()
index = -1



header = RangingClient.TDistanceHeader()
array = RangingClient.TDistanceArray()
time.sleep(1)
ret = rc.GetDistanceArray(array, header, tStamp, index, 1000)

print ret.__len__()
#Elements in ret are
#[0] = boolean ret value
#[1] = array
#[2] = ?index?
if ret.__len__() == 3:
        print "Index is", ret[2]
        print "Index is ",index #This seems not to work
	print "distance of scan 10 of " + ret[1].__len__().__str__() + " scans is " + ret[1][10].distance.__str__()
	print "distance of scan 11 of %i scans is %fm pointing at %frad with beam width %frad" % \
	(header.beam_count, array[11].distance, array[11].angle, header.beam_width)
	print "data from time instance %i.%.6is"%(tStamp.timestamp_s, tStamp.timestamp_us)

#If you want to wait for the next array you have to put the ret[2] value to index
index = ret[2]
ret = rc.GetDistanceArray(array, header, tStamp, index, 1000)

print ret.__len__()
if ret.__len__() == 3:
        print "Index is", ret[2]
        print "Index is ",index #This seems not to work
	print "distance of scan 10 of " + ret[1].__len__().__str__() + " scans is " + ret[1][10].distance.__str__()
	print "distance of scan 11 of %i scans is %fm pointing at %frad with beam width %frad" % \
	(header.beam_count, array[11].distance, array[11].angle, header.beam_width)
	print "data from time instance %i.%.6is"%(tStamp.timestamp_s, tStamp.timestamp_us)

time.sleep(1.0)
ret = rc.GetDistanceArray(array, header, tStamp, index, 1000)

print ret.__len__()
if ret.__len__() == 3:
        print "Index is", ret[2]
        print "Index is ",index #This seems not to work
	print "distance of scan 10 of " + ret[1].__len__().__str__() + " scans is " + ret[1][10].distance.__str__()
	print "distance of scan 11 of %i scans is %fm pointing at %frad with beam width %frad" % \
	(header.beam_count, array[11].distance, array[11].angle, header.beam_width)
	print "data from time instance %i.%.6is"%(tStamp.timestamp_s, tStamp.timestamp_us)


limits = RangingClient.TDeviceInformationLimits()
ret = rc.GetDeviceLimits(limits, 1000)
if ret:
	print "measurement range is between %f and %f "%(limits.max_distance, limits.min_distance)

pose = RangingClient.TDeviceInformationPosition()
ret = rc.GetDevicePosition(pose, 1000)
if ret:
	print "Device at (%f,%f,%f) pointing at %f"%(pose.x, pose.y, pose.z, pose.heading)

print "closing...",
ret = rc.Close()
print RangingClient.GetErrorStr(ret)
print "stopping...",
ret = g.stop()
print ret
